#include <iomacros.h>
#include <msp430f2617.h>
#include <stdarg.h>
#include <stdio.h>

#include "drivers/uart.h"
#include "drivers/dbg_uart.h"
#include "drivers/spi.h"
#include "drivers/cc2420.h"
#include "common.h"
#include "fmux.h"
#include "receive.h"
#include "error.h"


#define DELTA_1MHZ		244				// 244 x 4096Hz = 999.4Hz
#define DELTA_8MHZ		1953			// 1953 x 4096Hz = 7.99MHz
#define DELTA_12MHZ		2930			// 2930 x 4096Hz = 12.00MHz
#define DELTA_16MHZ		3906			// 3906 x 4096Hz = 15.99MHz

/* Indicate if a task needs to be called */
extern uint8_t read_task;
extern uint8_t cmd_task;

extern union cmd_line_t cmd_line; 

/* TODO: copyright? */
void set_DCO(unsigned int delta)
{
	unsigned int cmp = 0;
	unsigned int oldcmp = 0;

	BCSCTL1 &= ~BV(6);
	BCSCTL1 |= DIVA_3;						// ACLK = LFXT1CLK/8
	TACTL = TACLR;							// clear
	TACCTL2 = CM_1 | CCIS_1 | CAP | CCIE;	// CAP, ACLK
	TACTL = TASSEL_2 | MC_2;				// SMCLK, cont-mode

	while (1) {
		while (!(CCIFG & TACCTL2));			// Wait until capture occured

		TACCTL2 &= ~CCIFG;					// Capture occured, clear flag
		cmp = TACCR2;						// Get current captured SMCLK
		cmp = cmp - oldcmp;					// SMCLK difference
		oldcmp = TACCR2;						// Save current captured SMCLK

		if (delta == cmp) {
			goto end;							// If equal, leave "while(1)"
		} else if (delta < cmp) {
			DCOCTL--;						// DCO is too fast, slow it down
			if (DCOCTL == 0xFF) {			// Did DCO roll under?
				if (BCSCTL1 & 0x0f) {
					BCSCTL1--;				// Select lower RSEL
				}
			}
		} else {
			DCOCTL++;						 // DCO is too slow, speed it up
			if (DCOCTL == 0x00) {			 // Did DCO roll over?
				if ((BCSCTL1 & 0x0f) != 0x0f) {
					BCSCTL1++;				// Sel higher RSEL
				}
			}
		}
	}
end:
	TACCTL2 = 0;							// Stop TACCR2
	TACTL = 0;								// Stop Timer_A
	BCSCTL1 &= ~DIVA_3;						// ACLK = LFXT1CLK
}

int main(void)
{
	WDTCTL = (WDTPW | WDTHOLD); // Stop WDT
	/* red led */
	P5DIR |= BV(4);
	P5OUT |= BV(4);
	/* blue led */
	P5DIR |= BV(5);
	P5OUT |= BV(5);
	/* green led */
	P5DIR |= BV(6);
	P5OUT |= BV(6);
 

	__delay_cycles(2*0xfffe);				// Delay for XTAL stabilization
	P1OUT = 0x00;							// Clear P1 output latches
	P1DIR = 0x01;							// P1.0 output
	P2SEL |= 0x02;							// P2.1 SMCLK output
	P2DIR |= 0x02;							// P2.1 output

	P5OUT &= ~BV(5);
	set_DCO(DELTA_8MHZ);
	P5OUT |= BV(5);

	P5OUT &= ~BV(6);

	/* init */
	uart_enable();
	spi_init();
	cc2420_init();

	__enable_interrupt();

	while(1) {
		process_receive();

		if (read_task == 1)
		{
			uint8_t frame[128];
			uint8_t len;
			read_task = 0;

			cc2420_read_rxfifo();
			len = cc2420_get_frame(&(frame[0]));

			/* Skip the checksum */
			uart_send_comm(&(frame[1]), len);
		}

		if (cmd_task == 1)
		{
			cmd_task = 0;
			//recv_process(&(cmd_line.cmd));
		}
	}
	return 0;
}
